/*
 * GridWorld.cc
 *
 *  Created on: 11 jan. 2011
 *      Author: Frans
 */

#include "GridWorld.h"
#include <cstddef>
#include <stdio.h>
#define GOAL_REWARD 20.0;
#define WALL_COLLISION_REWARD -2.0;
#define STANDARD_REWARD -1.0;

GridWorld::GridWorld(int x, int y, double alfa, double gamma, Coordinate *goal,
		Coordinate *start) {
	this->initGridWorld(x, y);
	this->alfa = alfa;
	this->gamma = gamma;
	this->start = this->getState(start);
	this->goal = this->getState(goal);
	this->current = this->start;
}

void GridWorld::initGridWorld(int x, int y) {
	for (int i = 0; i < x; i++) {
		for (int j = 0; j < y; j++) {
			states[i][j] = new State(i, j);
		}
	}
	states[0][6] = NULL;
	states[0][3] = NULL;
	states[0][0] = NULL;
	states[1][3] = NULL;
	states[3][6] = NULL;
	states[3][5] = NULL;
	//states[3][3] = NULL;
	states[3][1] = NULL;
	states[3][0] = NULL;
	states[5][3] = NULL;
	states[6][6] = NULL;
	states[6][3] = NULL;
	states[6][0] = NULL;
}

void GridWorld::takeStep(EpsilonPolicy *policy) {
	int direction = policy->getPreferredDirection(
			this -> getCurrentState()-> getQValueNorth(),
			this -> getCurrentState()-> getQValueEast(),
			this -> getCurrentState()-> getQValueSouth(),
			this -> getCurrentState()-> getQValueWest());

	int x = this->getCurrentState()->getX() + Direction::getXDisplacement(
			direction);
	int y = this->getCurrentState()->getY() + Direction::getYDisplacement(
			direction);
	Coordinate *coordinate = new Coordinate(x, y);

	State *newState = this->getState(coordinate, direction);

	double reward;
	if (newState == this->goal) {
		reward = GOAL_REWARD;
	} else if (newState == this->current) {
		reward = WALL_COLLISION_REWARD;
	} else {
		reward = STANDARD_REWARD;
	}

	double newQValue = this->calculateNewQValue(newState, direction, reward);
	this->getCurrentState()->setQValue(direction, newQValue);
	this->setCurrentState(newState);
}

State * GridWorld::getState(Coordinate *coordinate) {
	int x = coordinate->getX();
	int y = coordinate->getY();
	if (x < 0) {
		return NULL;
	} else if (x > 6) {
		return NULL;
	}
	if (y < 0) {
		return NULL;
	} else if (y > 6) {
		return NULL;
	}
	return this->states[x][y];
}

//Geeft state op gegeven coordinaat terug. Als dat resultaat nul (op muur bebotst) zou zijn, geeft het de vorige
//state terug.
State * GridWorld::getState(Coordinate *coordinate, int direction) {
	State *state = this->getState(coordinate);
	if (state == NULL) {
		int x = Direction::getXDisplacement(direction);
		int y = Direction::getYDisplacement(direction);
		Coordinate *oldCoordinate = new Coordinate(coordinate->getX() - x,
				coordinate->getY() - y);
		state = this->getState(oldCoordinate);
	}
	return state;
}

void GridWorld::setCurrentState(State *state) {
	this->current = state;
}

double GridWorld::calculateNewQValue(State *newState, int direction, int reward) {
	double result = this->getCurrentState()->getQValue(direction);
	double maxQValue = this->getMaxQValue(newState);
	maxQValue *= this->gamma;
	maxQValue += reward;
	maxQValue -= this->getCurrentState()->getQValue(direction);
	maxQValue *= this->alfa;
	result += maxQValue;
	return result;
}

double GridWorld::getMaxQValue(State *newState) {
	double result = this->getCurrentState()->getQValue(0);
	for (int i = 1; i < 4; i++) {
		if (this->getCurrentState()->getQValue(i) > result) {
			result = this->getCurrentState()->getQValue(i);
		}
	}
	return result;
}

State *GridWorld::getCurrentState() {
	return this->current;
}

State *GridWorld::getGoalState() {
	return this->goal;
}

bool GridWorld::isInGoal() {
	if ((this->getCurrentState()->getX() == this->getGoalState()->getX())
			&& (this->getCurrentState()->getY() == this->getGoalState()->getY())) {
		return true;
	}
	return false;
}

void GridWorld::reset() {
	this->current = this->start;
}

void GridWorld::bestDirection() {
	printf("***Directions*** \n");
	for (int i = 6; i >= 0; i--) {
		for (int j = 0; j < 7; j++) {
			State *state = states[j][i];
			if (state == NULL) {
				printf("NULL  ");
			} else {
				int direction = state -> bestDirection();
				if (direction == 0) {
					printf("NORTH ");
				} else if (direction == 1) {
					printf("EAST  ");
				} else if (direction == 2) {
					printf("SOUTH ");
				} else {
					printf("WEST  ");
				}
			}
		}
		printf("\n");
	}
}

void GridWorld::printQValues() {
	printf("***QValues*** \n");
	for (int i = 6; i >= 0; i--) {
		for (int j = 0; j < 7; j++) {
			State *state = states[j][i];
			if (state == NULL) {
				printf("(X, X, X, X) ");
			} else {
				printf("(");
				printf("%f, ",state->getQValue(0));
				printf("%f, ",state->getQValue(1));
				printf("%f, ",state->getQValue(2));
				printf("%f",state->getQValue(3));
				printf(") ");
			}
		}
		printf("\n");
	}
	printf("\n");
}
